#include <jni.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <vector>
#include <stdio.h>
#include <android/log.h>

__android_log_write(ANDROID_LOG_ERROR, "Tag", "Error here");

using namespace std;
using namespace cv;

extern "C" {

using namespace cv;
using namespace std;

std::vector<cv::KeyPoint> keypoints;

Point2f pt;
vector<Point2f> points[2];
int needToInitC;
  

JNIEXPORT int JNICALL Java_org_opencv_samples_tutorial4_Sample4View_getNeedToInit(JNIEnv* env, jobject thiz)
{
	return needToInitC;
}



JNIEXPORT void JNICALL Java_org_opencv_samples_tutorial4_Sample4View_lucasKanade(JNIEnv* env, jobject thiz, jlong addrGray, jlong addrRgba, jlong addrPreviousGray, jint needToInit, jint deleteAll)
{
	const int MAX_COUNT  = 500;
    Mat* gray =(Mat*)addrGray;
    Mat* prevGray =(Mat*)addrPreviousGray;
    Mat* image =(Mat*)addrRgba;
	
	TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03);
	Size subPixWinSize(10,10), winSize(31,31);
    
    needToInitC = needToInit;		
    if (needToInit)
    {
		// automatic initialization
        goodFeaturesToTrack(*gray, points[1], MAX_COUNT, 0.01, 10, Mat(), 3, 0, 0.04);
        cornerSubPix(*gray, points[1], subPixWinSize, Size(-1,-1), termcrit);   		
    }
    else if (!points[0].empty())
    {
    	vector<uchar> status;
        vector<float> err;
        calcOpticalFlowPyrLK(*prevGray, *gray, points[0], points[1], status, err, winSize, 3, termcrit, 0, 0, 0.001);
        size_t i, k;
        for( i = k = 0; i < points[1].size(); i++ )
        {
            if( !status[i] )
                continue;

            points[1][k++] = points[1][i];
            circle( *image, points[1][i], 3, Scalar(255,0,0,255));
        }
        points[1].resize(k);
    } 
    	
	if( points[1].size() < (size_t)MAX_COUNT )
    {
        vector<Point2f> tmp;
        tmp.push_back(pt);
        cornerSubPix( *gray, tmp, winSize, cvSize(-1,-1), termcrit);
        points[1].push_back(tmp[0]);
    }
    	
    needToInit = 0;
    needToInitC = needToInit;
    
    if (deleteAll)
    {
        points[0].clear();
		points[1].clear();
	}
    	
    std::swap(points[1], points[0]);
}

}
